#define SHOW_GRAPHICS true

#include "ShowCollisionWorld.h"

//Coordinates systems are different for the kinematic model and the collision world, 
//while the x axis are the same, the kinematic Y axis is the same as the collision world Z axis 
//and the kinematic Z axis is the same as the collision world Y axis with opposite direction
#define STARTX SHOULDERX
#define STARTY -(SHOULDERZ - FIRST_LINK_LENGTH - SECOND_LINK_LENGTH - HAND_CENTER_OFFSET)
#define STARTZ SHOULDERY

#define NUMBER_OF_TRAJECTORY_POINTS 20

//=============================
// Displaying cubes is very long!
// so we limit their numbers.
 const int MAX_DISPLAYED_CUBES(15000);
//=============================


dirKinResult dirKin(float teta1, float teta2, float teta3, float teta4);
invKinResult invKin(float x, float y, float z);
void move(float startX, float startY, float startZ, float objectX, float objectY, float objectZ, CollisionWorldViewer* CWV, vector<Point>* puntiDiVia);

 ShowCollisionWorld::ShowCollisionWorld (std::vector<std::string> cloud_path, double resolution, int argc, char** argv, std::vector< object > objects, int no) :
    cloud (new pcl::PointCloud<pcl::PointXYZ>()), octree (resolution)
  {


	double minY, minX, maxX, avg[3] = {0,0,0};
	int i;
	Point* centroid;
	//double graspX, graspY, graspZ;
	double alpha = -1;
	//double beta;

	displayedDepth = static_cast<int> (floor ((float) octree.getTreeDepth()));
    if (displayedDepth == 0)
      displayedDepth = 1;

    //.pcd file loading
	if (!loadCloud(cloud_path.at(no)))
      return;


	std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ> > centri;
	octree.getOccupiedVoxelCenters(centri);


	//resolution = cubes' size in the collision world viewer
	CollisionWorldViewer CWV(resolution*(SCALING/2));
	

	//Calculating the average x, y and z coordinates to center the camera on the object
	minY=-centri.at(0).y;
	minX=centri.at(0).x;
	maxX= minX;
	int j = 0;
	for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centri.begin() ; it != centri.end(); ++it){
		if(minY > -it->y)
			minY = -it->y;

		if(minX > it->x)
			minX = it->x;
		
		if(maxX < it->x)
			maxX = it->x;
		
		avg[0] += it->x;
		avg[1] += it->y;
		avg[2] += it->z;

		j++;
	}
	
	avg[0] /= j;
	avg[1] /= j;
	avg[2] /= j;
	
	CWV.initPhysics(SCALING*avg[0], SCALING*avg[1], SCALING*avg[2]);
	CWV.addGround(SCALING*(minY - CWV.cubeSize));

	
	//Voxels creation cycle
	for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centri.begin() ; it != centri.end(); ++it)
		CWV.addDefaultSizedRigidBody(SCALING*(it->x),SCALING*(it->y),SCALING*(it->z));
	
	CWV.addSphere(objects.at(no).centroidX*SCALING, objects.at(no).centroidY*SCALING, objects.at(no).centroidZ*SCALING, resolution*SCALING);
	/*CWV.addSphere(maxX*SCALING, objects.at(no).centroidY*SCALING, objects.at(no).centroidZ*SCALING, resolution*SCALING);
	CWV.addSphere(minX*SCALING, objects.at(no).centroidY*SCALING, objects.at(no).centroidZ*SCALING, resolution*SCALING);*/

	
	centroid = new Point(objects.at(no).centroidX, objects.at(no).centroidY, objects.at(no).centroidZ);
	
	
	for(i = 0; i < cloud_path.size(); i++){

		if(i != no){

			//.pcd file loading
			if (!loadCloud(cloud_path.at(i)))
			return;

			octree.getOccupiedVoxelCenters(centri);

			minY=-centri.at(0).y;
			minX=centri.at(0).x;
			maxX= minX;
			
			for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centri.begin() ; it != centri.end(); ++it){
				if(minY > -it->y)
					minY = -it->y;

				if(minX > it->x)
					minX = it->x;
		
				if(maxX < it->x)
					maxX = it->x;
			}

			//Voxels creation cycle
			for (std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>>::iterator it = centri.begin() ; it != centri.end(); ++it)
				CWV.addDefaultSizedRigidBody(SCALING*(it->x),SCALING*(it->y),SCALING*(it->z));
	
			CWV.addSphere(objects.at(i).centroidX*SCALING, objects.at(i).centroidY*SCALING, objects.at(i).centroidZ*SCALING, resolution*SCALING);
			/*CWV.addSphere(minX*SCALING, objects.at(i).centroidY*SCALING, objects.at(i).centroidZ*SCALING, resolution*SCALING);
			CWV.addSphere(maxX*SCALING, objects.at(i).centroidY*SCALING, objects.at(i).centroidZ*SCALING, resolution*SCALING);*/


		}

	}
	
	
	vector<Point> myPath;

	move(STARTX, STARTY, STARTZ, centroid->getValue(Point::X), centroid->getValue(Point::Y), -centroid->getValue(Point::Z), &CWV, &myPath);
	 
	std::cout << "\nNumber of steps: " << myPath.size() << std::endl;
	this->setPath(myPath);
	std::cout << "\nLast point x: " << this->path.at(this->path.size() - 1).getValue(Point::X) << std::endl;



	#ifdef CHECK_MEMORY_LEAKS
		ccdDemo.exitPhysics();
	#else
		if(SHOW_GRAPHICS)
			glutmain(argc, argv,1024,600,"Collision world",&CWV);
	#endif



}




  /* \brief Helper function that read a pointcloud file (returns false if pbl)
   *  Also initialize the octree
   *
   */
  bool ShowCollisionWorld::loadCloud(std::string &filename)
  {

	pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> *octreeAux = new pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>(0.005);
	
    //read cloud
    if (pcl::io::loadPCDFile(filename, *cloud))
    {
      std::cerr << "ERROR: Cannot open file " << filename << "! Aborting..." << std::endl;
      return false;
    }

    //remove NaN Points
    std::vector<int> nanIndexes;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, nanIndexes);
    //std::cout << "Loaded " << cloud->points.size() << " points" << std::endl;

	
    //create octree structure
    octreeAux->setInputCloud(cloud);
    //update bounding box automatically
    octreeAux->defineBoundingBox();
    //add points in the tree
    octreeAux->addPointsFromInputCloud();

	octree = *octreeAux;

    return true;
  }

  std::vector<Point> ShowCollisionWorld::getPath(){
	  return this->path;
  }

  void ShowCollisionWorld::setPath(vector<Point> myPath){
	  this->path = myPath;
  }

 void move(float startX, float startY, float startZ, float objectX, float objectY, float objectZ, CollisionWorldViewer* CWV, vector<Point> *puntiDiVia){


	invKinResult ikr1, ikr2;
	dirKinResult dkr;
	float incTeta1, incTeta2, incTeta3;
	double xc, yc, zc;
	int flag;
	Point* currentPoint;

	/*float a,b,c;
	printf("teta1: ");
	scanf("%f",&a);
	printf("teta2: ");
	scanf("%f",&b);
	printf("teta3: ");
	scanf("%f",&c); 

	dkr = dirKin(a,b,c,0);
	std::cout << "\n\nx: " << dkr.handCenter.x << "\ny: " << dkr.handCenter.y << "\nz: " << dkr.handCenter.z << std::endl;
	*/

	//ikr = invKin(dkr.handCenter.x, dkr.handCenter.y, dkr.handCenter.z);


	//Incremental value calculation
	ikr1 = invKin(objectX, -objectZ, -objectY);
	ikr2 = invKin(startX, startZ, -startY);
	
	incTeta1 = (ikr1.teta1 - ikr2.teta1) / NUMBER_OF_TRAJECTORY_POINTS;
	incTeta2 = (ikr1.teta2 - ikr2.teta2) / NUMBER_OF_TRAJECTORY_POINTS;
	incTeta3 = (ikr1.teta3 - ikr2.teta3) / NUMBER_OF_TRAJECTORY_POINTS;

	
	std::cout << "\n\nx: " << objectX << "\ny: " << -objectZ << "\nz: " << -objectY << std::endl;
	std::cout << "\n\nx: " << startX << "\ny: " << startZ << "\nz: " << -startY << std::endl;
	std::cout << "\n\nteta1: " << ikr1.teta1 << "\nteta2: " << ikr1.teta2 << "\nteta3: " << ikr1.teta3 << std::endl;
	std::cout << "\n\nteta1: " << ikr2.teta1 << "\nteta2: " << ikr2.teta2 << "\nteta3: " << ikr2.teta3 << std::endl;
	std::cout << "\nincTeta1: " << incTeta1 << "\nincteta2: " << incTeta2 << "\nincteta3: " << incTeta3 << std::endl;
	
	flag = 1;
	//Collision checking loop
	int i;
	for(i = 0; i <= 20; i++){
		
		dkr = dirKin(ikr2.teta1 + incTeta1*i, ikr2.teta2 + incTeta2*i, ikr2.teta3 + incTeta3*i, 0);  
	
		xc = (dkr.handCenter->getValue(Point::X) - dkr.wrist->getValue(Point::X)) / HAND_CENTER_OFFSET;
		yc = (dkr.handCenter->getValue(Point::Y) - dkr.wrist->getValue(Point::Y)) / HAND_CENTER_OFFSET;
		zc = (dkr.handCenter->getValue(Point::Z) - dkr.wrist->getValue(Point::Z)) / HAND_CENTER_OFFSET;
	

		flag = CWV->addHand(		dkr.handCenter->getValue(Point::X)*SCALING, dkr.handCenter->getValue(Point::Y)*SCALING, dkr.handCenter->getValue(Point::Z)*SCALING, 
									xc,yc,zc,
									dkr.graspingPoint1->getValue(Point::X)*SCALING, -dkr.graspingPoint1->getValue(Point::Z)*SCALING, dkr.graspingPoint1->getValue(Point::Y)*SCALING,
									dkr.graspingPoint2->getValue(Point::X)*SCALING, -dkr.graspingPoint2->getValue(Point::Z)*SCALING, dkr.graspingPoint2->getValue(Point::Y)*SCALING, 
									FINGER_SIZE*SCALING, FINGER_LENGTH*SCALING, GRASP_WIDTH*SCALING, HAND_HEIGHT*SCALING, HAND_LENGTH*SCALING,
									(ikr2.teta1 + incTeta1*i)*M_PI/180, (ikr2.teta2 + incTeta2*i)*M_PI/180, (ikr2.teta3 + incTeta3*i)*M_PI/180);


		if(flag == 0){
			std::cout << "\nCOLLISION DETECTED: " << flag << std::endl;
			std::cout << "\nSTEP: " << i << std::endl;

			std::cout <<	"\n\nPosition:"
						"\n   X:\t" << objectX <<
						"\n   Y:\t" << objectY <<
						"\n   Z:\t" << objectZ << std::endl;

		}
		else{
			std::cout << "\nNO COLLISION: " << flag << std::endl;
		}

		//std::cin >> flag;
		
		currentPoint = new Point(dkr.handCenter->getValue(Point::X), dkr.handCenter->getValue(Point::Y), dkr.handCenter->getValue(Point::Z));

		puntiDiVia->push_back(*currentPoint);

		if(flag == 1 && i != 20)
			CWV->removeHand();
		else
			break;


	}

}

